#include "xbuffer.h"
#include "global.h"
#include "avr/io.h"

#ifndef CRIT_SECT_START
#define CRIT_SECT_START	unsigned char _sreg = SREG; cli()
#define CRIT_SECT_END	SREG = _sreg
#endif

void bufferInit(xBuffer*  buffer,  unsigned char* start,  unsigned short size){
    buffer->pData     = start;
    buffer->bufSize   = size;
    buffer->bytesCount = 0;
    buffer->iData     = 0;
}

unsigned short bufferRead(xBuffer*  buffer,  unsigned char* dest,   unsigned short size){
    unsigned short bytesRead;
    bytesRead = 0;

    CRIT_SECT_START;
    //Is there enough data?
    if(buffer->bytesCount >= size){
        //Copy block of data
        for(bytesRead = 0; bytesRead < size; bytesRead++){
            dest[bytesRead] = buffer->pData[ (buffer->iData+bytesRead)%(buffer->bufSize) ];
        }
        buffer->iData = (buffer->iData + bytesRead) % (buffer->bufSize);
        buffer->bytesCount = buffer->bytesCount - size;
    }
    CRIT_SECT_END;
    return bytesRead;
}

unsigned short bufferWrite(xBuffer* buffer,  unsigned char* source, unsigned short size){
    unsigned short bytesWritten;
    bytesWritten = 0;

    CRIT_SECT_START;
    //Is there enough space?
    if( (buffer->bufSize-buffer->bytesCount) >= size ){
        for(bytesWritten = 0; bytesWritten < size; bytesWritten++){
            buffer->pData[ (buffer->iData + buffer->bytesCount + bytesWritten) % (buffer->bufSize) ] = source[bytesWritten];
        }
        buffer->bytesCount = buffer->bytesCount + size;
    }
    CRIT_SECT_END;
    return bytesWritten;
}

void bufferFlush(xBuffer* buffer){
    CRIT_SECT_START;
    buffer->bytesCount = 0;
    CRIT_SECT_END;
    return;
}

unsigned short bufferGetCount(xBuffer* buffer){
    unsigned short bytesCount;

    CRIT_SECT_START;
    bytesCount = buffer->bytesCount;
    CRIT_SECT_END;
    return bytesCount;
}

unsigned short bufferGetBytesLeft(xBuffer* buffer){
    unsigned short bytesLeft;

    CRIT_SECT_START;
    bytesLeft = buffer->bufSize - buffer->bytesCount;
    CRIT_SECT_END;

    return bytesLeft;
}
